package org.bmo;
import lejos.nxt.ColorLightSensor;
import lejos.robotics.Colors;

public class scanner extends Thread{
	
	private ColorLightSensor myColorLightSensor;
   // private UnterArm myUnterArm;
    private int[][] myColors = new int[6][3];
    //private OberArm myOberArm;
    private float thePI=(float) Math.PI;
    private boolean[][] colorMatrix = new boolean[80][200];
    private String[] colors2Numbers = new String[6]; 
    protected boolean threatShouldStop = false;
    private byte[] scannedNumbers = new byte[2];
    
    private int[][] hex2Numbers = new int[6][3]; 
    
	public scanner(/*OberArm OberArm1, UnterArm UnterArm1,*/ ColorLightSensor colorSensor){
		myColorLightSensor = colorSensor;
		//myOberArm = OberArm1;
		//myUnterArm = UnterArm1;
		
		colors2Numbers[0] = "GREEN";
		colors2Numbers[1] = "WHITE";
		colors2Numbers[2] = "BLUE";
		colors2Numbers[3] = "RED";		
		colors2Numbers[4] = "BLACK";
		colors2Numbers[5] = "YELLOW";
		
	}

	public void run() {

		//Sensor
		myColorLightSensor.setFloodlight(true);
		myColorLightSensor.setType(ColorLightSensor.TYPE_COLORFULL + 0);
		myColorLightSensor.setHigh(MAX_PRIORITY);
		myColorLightSensor.setLow(MIN_PRIORITY);
        
		int rawVals[] = new int[4];
        int foundNumbersCount[] = new int[6];
		
        while (!this.isClosing()) {
						
            Colors.Color color = myColorLightSensor.readColor();
            myColorLightSensor.readRawValues(rawVals);
            
             int scannedNr = 0;
             for(int i = 0; i < colors2Numbers.length; i = i + 1){
    			//LCD.drawString("CC -" + (String)color.name() + "-" + colors2Numbers[i] + "-", 0, i);            		
                //LCD.drawString("ROH :" + rawVals[0], 0, 1);
                //LCD.drawString("C :" + color.name(), 0, 2);
                //LCD.drawString("R1 :" + rawVals[0], 0, 3);

                
            	if(color.name().equals("YELLOW") && rawVals[0] < 520)
            		break;

            	if(color.name().equals("BLACK") && rawVals[0] > 400)
            		break;            	
            	
                if(colors2Numbers[i].equals(color.name() ) ){                	
                	foundNumbersCount[i]++;
                }
            }//End for
            
            if(scannedNr == 0)
            	continue;
		}
        
        
        
        int biggest1 = 0;
        int biggest2 = 0;
        byte biggest1Number = 0;
        byte biggest2Number = 0;
        int minFoundValue = 30;
        
        for(int i = 0; i < foundNumbersCount.length; i = i + 1){
        	if(foundNumbersCount[i] > biggest1 && foundNumbersCount[i] >= minFoundValue){
        		biggest1 = foundNumbersCount[i];
        		biggest1Number = (byte) i;
        	}        		
        }

        for(int i = 0; i < foundNumbersCount.length; i = i + 1){
        	if(foundNumbersCount[i] > biggest2 && i != biggest1Number && foundNumbersCount[i] >= minFoundValue){
        		biggest2 = foundNumbersCount[i];
        		biggest2Number = (byte) i;
        	}        		
        }
		
		
		if(biggest1 > 0 && biggest2 == 0){
			scannedNumbers[0] = (byte) (biggest1Number + 1);
			scannedNumbers[1] = (byte) (biggest1Number + 1);
		}
		else if(biggest1 > 0 && biggest2 > 0){
			scannedNumbers[0] = (byte) (biggest1Number + 1);
			scannedNumbers[1] = (byte) (biggest2Number + 1);
		}
		else{
			//error
		}
		
      /*  LCD.drawString("FNC 1 : " + foundNumbersCount[0], 0, 1);
        LCD.drawString("FNC 2 : " + foundNumbersCount[1], 0, 2);
        LCD.drawString("FNC 3 : " + foundNumbersCount[2], 0, 3);
        LCD.drawString("FNC 4 : " + foundNumbersCount[3], 0, 4);
        LCD.drawString("FNC 5 : " + foundNumbersCount[4], 0, 5);
        LCD.drawString("FNC 6 : " + foundNumbersCount[5], 0, 6);
		LCD.drawString("SN: " + scannedNumbers[0] + " " + scannedNumbers[1], 0, 7);*/
		try {
			sleep(10000);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			//e.printStackTrace();
		}
	}	
	
	
	public byte[] get_scannedNumbers(){
		return scannedNumbers;
	}
	

	public synchronized void close() {
		this.threatShouldStop = true;
	}
	
	protected synchronized boolean isClosing() {
		return this.threatShouldStop;
	}
	
	
}
